#pragma once
#include "QSocketServer.h"
#include "../../Project/KT/TestJsonData.h"
#include "../../Project/SQ/Util.h"
#include "../../Project/SSQK/Util.h"
#include "../../ModelMgr.h"
#include "../../Camera.h"
#include "../../core/CUtil.h"
#include "../../core/CElapsedTimer.h"
#include "QDynQml.h"
#include <QDebug>

namespace sq {
	const QString PkgBeginStr = "23 23 10 20";
	struct PkgHead {
		quint32 begin;
		quint16 no;
		quint16 cmd;
		quint16 byteCnt;
		quint8 cnt;

		quint8 src;// just for Line, if Target ignore this field!

		bool isMapBox() const { return src == 0; }
	};
}

namespace ssqk {
	const QString PkgBeginStr = "AA 55 06 D0";
	struct PkgHead {
		quint16 begin;// AA 55
		quint8 groupId;// 06
		quint8 cmdId;// D0
		quint16 msgLen;
		quint16 pkgNum;
		quint16 pkgNO;

		quint8 dataType;
	};
}

class QTestCase {
public:
	static void run() {
		Cache::updateZhicheSpeed(15);// give a speed 15km/h to zhiche for test

		const static bool UsingRawBits = true;
		const static bool RunRawBitsJustOnce = false;
		const static float StartDataPercent = 0.0;
		const static int DelayNFrames = 0;// example: 10s = 60 * 10
		if (UsingRawBits && !Config::TestCase_SampleDataEnabled) {
			QString logPath = R"(D:\LOG\ALogTmp\SQ_10-22)";
			QString logFileName = "system-2025-10-22-12-15-38-22.log";

			qDebug() << "reading *.log.bin ......";
			QDynQml::get()->setDebugMsg("reading *.log ......");
			static QStringList allBitsStrs, allBitsTimes;
			bool success = readFileToStringList(allBitsStrs, allBitsTimes, logPath + "/" + logFileName);
			QDynQml::get()->setDebugMsg("reading *.log done! total count = " + QString::number(allBitsStrs.size()));

			if (success) {
				qDebug() << "reading done! start rendering ......";
				static CElapsedTimer timer;
				static int frameCnt = 0;
				CUtil::loop([&] {//note: do not use qDebug() to print msg: may leading to 'Cannot send events to objects owned by a different thread'
					if (++frameCnt < DelayNFrames)
						return;
					static int i = allBitsStrs.size() * StartDataPercent - 1;

					const auto& bitsStr = allBitsStrs[++i];
					const auto& bitsTime = allBitsTimes[i];
					QString msg;
					msg += "[" + bitsTime + "]\n";

					/** @note linesBits and modelsBits pkg data is received one by one, not parallel!*/
					static QString lastLinesNumStr, lastTargetsNumStr;
					static int noLinesCnt = 0, noTargetsCnt = 0;

					CVector<LineArg> lines;
					CVector<ModelArg> models;
					if (Proj::isSQ()) {
						static QString lastLinesFlowNO, lastTargetsFlowNO;
						if (i == -1 && (!lastLinesFlowNO.isEmpty() || !lastTargetsFlowNO.isEmpty())) {
							if (RunRawBitsJustOnce)// just run once
								return;
							lastLinesFlowNO = lastTargetsFlowNO = "";
							return;
						}
						sq::PkgHead head;
						parseABlockBitsStr(head, lines, models, bitsStr);

						auto flowNO = QString("%1 %2").arg((head.no >> 8) & 0xFF, 2, 16, QChar('0')).arg(head.no & 0xFF, 2, 16, QChar('0')).toUpper();
						if (!models.empty()) {
							lastTargetsFlowNO = flowNO;
							lastTargetsNumStr = " (" + QString::number(models.size()) + ")";
							noTargetsCnt = 0;
						} else
							++noTargetsCnt;
						if (!lines.empty()) {
							lastLinesFlowNO = flowNO;
							lastLinesNumStr = " (" + QString::number(lines.size()) + ")";
							noLinesCnt = 0;
						} else
							++noLinesCnt;

						if (noTargetsCnt < 3)
							msg += "Targets: #" + (lastTargetsFlowNO.isEmpty() ? "No Targets!" : lastTargetsFlowNO + lastTargetsNumStr) + "\n";
						if (noLinesCnt < 3)
							msg += "Lines: #" + (lastLinesFlowNO.isEmpty() ? "No Lines!" : lastLinesFlowNO + lastLinesNumStr) + "\n";
					} else if (Proj::isSSQK()) {
						static bool sIsFirstTime = true;
						if (!sIsFirstTime && i == -1 && RunRawBitsJustOnce)
							return;
						sIsFirstTime = false;

						ssqk::PkgHead head;
						parseABlockBitsStr(head, lines, models, bitsStr);

						if (!models.empty()) {
							lastTargetsNumStr = " (" + QString::number(models.size()) + ")";
							noTargetsCnt = 0;
						} else
							++noTargetsCnt;
						if (!lines.empty()) {
							lastLinesNumStr = " (" + QString::number(lines.size()) + ")";
							noLinesCnt = 0;
						} else
							++noLinesCnt;

						if (noTargetsCnt < 3)
							msg += "Targets Num: " + lastTargetsNumStr + "\n";
						if (noLinesCnt < 3)
							msg += "Lines Num: " + lastLinesNumStr + "\n";
					}
					Cache::recvLines(lines);
					Cache::recvModels(models);

					msg += QString("(%1/%2)").arg(i + 1).arg(allBitsStrs.size());
					QDynQml::get()->setDebugMsg(msg);

					if (i == allBitsStrs.size() - 1) {
						i = -1;
						QDynQml::get()->setDebugMsg("Finished!!!!!!!!!!");
						static int round = 0;
						std::cout << "round #" << ++round << ", consumes: " << timer.elapsed() << "ms";
						timer.reset();
					}
				}, 16);
			}
		}

		TestJsonData::parsingInBgThread();

		startServerListening();
	}

	static void parseABlockBitsStr(sq::PkgHead& outHead, CVector<LineArg>& outLines, CVector<ModelArg>& outModels, QString bitsStr) {
		QByteArray bytes = QByteArray::fromHex(bitsStr.replace(" ", "").toLatin1());
		QDataStream stream(&bytes, QIODevice::ReadOnly);

		sq::PkgHead head;
		stream >> head.begin >> head.no >> head.cmd >> head.byteCnt >> head.cnt;
		if (head.cmd == 0x0501)
			stream >> head.src;
		outHead = head;

		static const bool ExtractLines = true;
		static const bool ExtractModels = true;

		if (head.cmd == 0x0501) {// Lines
			if (!ExtractLines)
				return;
			for (int i = 0; i < head.cnt; i++) {
				LineArg line;
				quint8 id, style, color;
				stream >> id >> style >> color;
				line.style = sq::Util::lineStyle(style);
				line.color = sq::Util::lineColor(color);
				if (!head.isMapBox()) {
					quint16 c0N, c1N, c2N, c3N;
					stream >> c0N >> c1N >> c2N >> c3N;
				
					line.eq.C << c0N * 0.05 - 50;
					line.eq.C << c1N * 0.01 - 100;
					line.eq.C << c2N * 0.0000078125 - 0.25;
					line.eq.C << c3N * 0.0000000625 - 0.002;

					line.eq.zStart = 1000;
					line.eq.zEnd = -1000;
				} else {
					quint8 pointsNum;
					stream >> pointsNum;
					for (int k = 0; k < pointsNum; k++) {
						qint16 vDistN, hDistN;
						stream >> vDistN >> hDistN;
						float hDist = hDistN * 0.05 - 50;
						float vDist = vDistN * 0.05 - 20;
						line.eq.points << sq::Util::modelPos(hDist, vDist);
					}
				}
				outLines << line;
			}
		} else if (head.cmd == 0x0500) {// Models
			if (!ExtractModels)
				return;
			int N = (head.byteCnt - 1) / 8;
			for (int i = 0; i < N; i++) {
				ModelArg model;
				quint8 id, type;
				quint16 vDistN, hDistN, yawAngleN;
				stream >> id >> type >> vDistN >> hDistN >> yawAngleN;
				float hDist = hDistN * 0.05 - 50;
				float vDist = vDistN * 0.05 - 20;
				float yawAngle = CMath::radians(yawAngleN * 1.0 - 180);

				model.ID = id;
				model.type = sq::Util::modelType(type);
				model.pos = sq::Util::modelPos(hDist, vDist);
				model.angle = yawAngle;

				outModels << model;
			}
		}
	}

	static void parseABlockBitsStr(ssqk::PkgHead& outHead, CVector<LineArg>& outLines, CVector<ModelArg>& outModels, const QString& bitsStr) {
		QString aBlockBitsStr = bitsStr;
		QByteArray bytes = QByteArray::fromHex(aBlockBitsStr.replace(" ", "").toLatin1());
		QDataStream stream(&bytes, QIODevice::ReadOnly);

		stream >> outHead.begin >> outHead.groupId >> outHead.cmdId >> outHead.msgLen >> outHead.pkgNum >> outHead.pkgNO >> outHead.dataType;

		static const bool ExtractLines = true;
		static const bool ExtractModels = true;

		if (outHead.dataType == 0x02) {// Lines
			if (!ExtractLines)
				return;
			quint16 speedLimit;
			quint8 accMode;
			quint8 lkaLaneKeep;
			quint8 steeringWheelState;
			stream >> speedLimit >> accMode >> lkaLaneKeep >> steeringWheelState;
			for (int i = 0; i < 4; i++) {// Left,Right,LelfLeft,RightRight
				LineArg line;
				quint8 style;
				quint8 color;
				stream >> style >> color;
				line.style = ssqk::Util::lineStyle(style);
				line.color = ssqk::Util::lineColor(color);
				outLines << line;
			}
			quint8 lDist;
			quint8 rDist;
			stream >> lDist >> rDist;
			float lC0 = lDist * 0.1 - 1;
			float rC0 = -(rDist * 0.1 - 5);// ToDo remove the negative sign when the MCU do the logic
			float llC0 = lC0 + 3.6;
			float rrC0 = rC0 - 3.6;

			outLines[0].eq.C.resize(4, 0);
			outLines[1].eq.C.resize(4, 0);
			outLines[2].eq.C.resize(4, 0);
			outLines[3].eq.C.resize(4, 0);

			outLines[0].eq.C[0] = lC0;
			outLines[1].eq.C[0] = rC0;
			outLines[2].eq.C[0] = llC0;
			outLines[3].eq.C[0] = rrC0;

			outLines[0].eq.zStart = 1000; outLines[0].eq.zEnd = -1000;
			outLines[1].eq.zStart = 1000; outLines[1].eq.zEnd = -1000;
			outLines[2].eq.zStart = 1000; outLines[2].eq.zEnd = -1000;
			outLines[3].eq.zStart = 1000; outLines[3].eq.zEnd = -1000;
		}
		else if (outHead.dataType == 0x01) {// Models
			if (!ExtractModels)
				return;
			quint8 N;
			stream >> N;
			for (int i = 0; i < N; i++) {
				ModelArg model;
				quint8 ID;
				quint8 type;
				quint8 hDistN;
				quint8 vDistN;
				quint8 recognize;
				quint8 vSpeedN;
				quint8 hSpeedN;
				quint16 yawAngle;// degree
				stream >> ID >> type >> hDistN >> vDistN >> recognize >> vSpeedN >> hSpeedN >> yawAngle;
				model.ID = ID;
				model.type = ssqk::Util::modelType(type);
				model.pos = ssqk::Util::modelPos(hDistN - 50, vDistN);
				model.angle = 0;//CMath::radians(yawAngle - 180);
				model.recog = recognize;
				model.relSpeed = Vec2(hSpeedN - 50, vSpeedN - 50);

				outModels << model;
			}
		}
	}

	static bool readFileToStringList(QStringList& outRawBitsList, QStringList& outRawBitsTimeList, const QString& logFilePath) {
		QFile file(logFilePath);
		if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
			qDebug() << "can not open file: " << logFilePath;
			return false;
		}
		QString pkgBeginStr;
		if (Proj::isSQ())
			pkgBeginStr = sq::PkgBeginStr;
		else if (Proj::isSSQK())
			pkgBeginStr = ssqk::PkgBeginStr;

		QTextStream in(&file);
		while (!in.atEnd()) {
			QString line = in.readLine();
			int tInd = line.indexOf(pkgBeginStr);
			bool isTargetFound = tInd != -1;
			if (!isTargetFound)
				continue;

			auto bitsStr = line.mid(tInd);
			outRawBitsList << bitsStr;

			QString timeStr;
			auto pre = line.mid(0, tInd);
			auto timeRegex = QRegularExpression(R"(\d{2}-\d{2}\s\d{2}:\d{2}:\d{2}\.\d{3})");// example: 10-22 12:15:38.123
			auto match = timeRegex.match(pre);
			if (match.hasMatch())
				timeStr = match.captured(0);
			outRawBitsTimeList << timeStr;
		}
		file.close();
	
		if (outRawBitsList.isEmpty()) {
			qDebug() << "file is empty: " << logFilePath;
			return false;
		}
		return true;
	}

	static void startServerListening() {
		static QSocketServer server;
		server.listenToHost([&](int feature, int iValue, float fValue) {
			ALC alc = ALC::INVALID;
			LDW ldw = LDW::INVALID;
			DCLC dclc = DCLC::INVALID;
			ACC_TG accTG = ACC_TG::INVALID;
			TailLamp tailLamp = TailLamp::INVALID;
			switch (feature) {
				// ALC
				case QSocketServer::Feature::ALC: {
					switch (iValue) {
						case 0: alc = ALC::Off; break;
						case 1: alc = ALC::Left_Normal; break;
						case 2: alc = ALC::Left_Warning; break;
						case 3: alc = ALC::Right_Normal; break;
						case 4: alc = ALC::Right_Warning; break;
					}
					break;
				}
				// LDW
				case QSocketServer::Feature::LDW: {
					switch (iValue) {
						case 0: ldw = LDW::Off; break;
						case 1: ldw = LDW::From_Left_Warning; break;
						case 2: ldw = LDW::From_Right_Warning; break;
					}
					break;
				}
				// DCLC
				case QSocketServer::Feature::DCLC: {
					switch (iValue) {
						case 0: dclc = DCLC::Off; break;
						case 1: dclc = DCLC::ToLeft; break;
						case 2: dclc = DCLC::ToRight; break;
						case 3: dclc = DCLC::Mid; break;
					}
					break;
				}
				// ACC_TG
				case QSocketServer::Feature::ACC_TG: {
					switch (iValue) {
						case 0: accTG = ACC_TG::Off; break;
						case 1: accTG = ACC_TG::Gear_First; break;
						case 2: accTG = ACC_TG::Gear_Second; break;
						case 3: accTG = ACC_TG::Gear_Third; break;
						case 4: accTG = ACC_TG::Gear_Fourth; break;
					}
					break;
				}
				// LampBrake
				case QSocketServer::Feature::LampBrake: {
					switch (iValue) {
						case 0: tailLamp = TailLamp::Off; break;
						case 1: tailLamp = TailLamp::Brake_On; break;
					}
					break;
				}
				// LampTurn
				case QSocketServer::Feature::LampTurn: {
					switch (iValue) {
						case 0: tailLamp = TailLamp::Off; break;
						case 1: tailLamp = TailLamp::Turn_Left; break;
						case 2: tailLamp = TailLamp::Turn_Right; break;
						case 3: tailLamp = TailLamp::Turn_Both; break;
					}
					break;
				}
				// Zhiche Door
				case QSocketServer::Feature::ZhicheDoor: {
					switch (iValue) {
						case 0: ModelMgr::get()->handleZhicheDoor(HDir::LEFT, Switch::ON); break;
						case 1: ModelMgr::get()->handleZhicheDoor(HDir::LEFT, Switch::OFF); break;
						case 2: ModelMgr::get()->handleZhicheDoor(HDir::RIGHT, Switch::ON); break;
						case 3: ModelMgr::get()->handleZhicheDoor(HDir::RIGHT, Switch::OFF); break;
					}
					break;
				}
				// Zhiche Speed
				case QSocketServer::Feature::ZhicheSpeed: {
					Cache::updateZhicheSpeed(iValue);
					break;
				}
				/* ************************* SQ **********************/
				/* ************************* SQ **********************/
				case QSocketServer::Feature::SQ_NOA: {
					switch (iValue) {
						case 0: Cache::updateSQ_NOA(sq::NOA::Off); break;
						case 1: Cache::updateSQ_NOA(sq::NOA::On); break;
					}
					return;
				}
				case QSocketServer::Feature::SQ_ALC: {
					switch (iValue) {
						case 0: Cache::updateSQ_ALC(sq::ALC::Off); break;
						case 1: Cache::updateSQ_ALC(sq::ALC::Left); break;
						case 2: Cache::updateSQ_ALC(sq::ALC::Right); break;
					}
					return;
				}
				case QSocketServer::Feature::SQ_ACAS: {
					switch (iValue) {
						case 0: Cache::updateSQ_ACAS(sq::ACAS::Off); break;
						case 1: Cache::updateSQ_ACAS(sq::ACAS::Left); break;
						case 2: Cache::updateSQ_ACAS(sq::ACAS::Right); break;
					}
					return;
				}
				case QSocketServer::Feature::SQ_ACC: {
					switch (iValue) {
						case 0: Cache::updateSQ_ACC(sq::ACC::Off); break;
						case 1: Cache::updateSQ_ACC(sq::ACC::Follow); break;
					}
					return;
				}
				case QSocketServer::Feature::SQ_FCW: {
					switch (iValue) {
						case 0: Cache::updateSQ_FCW(sq::FCW::Off); break;
						case 1: Cache::updateSQ_FCW(sq::FCW::Yellow); break;
						case 2: Cache::updateSQ_FCW(sq::FCW::Red); break;
					}
					return;
				}

				// Camera Arg Debug
				case QSocketServer::Feature::Camera_PosX: Camera::get()->setPosX(fValue); break;
				case QSocketServer::Feature::Camera_PosY: Camera::get()->setPosY(fValue); break;
				case QSocketServer::Feature::Camera_PosZ: Camera::get()->setPosZ(fValue); break;
				case QSocketServer::Feature::Camera_PitchAngle: Camera::get()->setPitchAngle(fValue); break;
				case QSocketServer::Feature::Camera_Fov: Camera::get()->setFov(fValue); break;
				case QSocketServer::Feature::Camera_YawAngle: Camera::get()->setYawAngle(fValue); break;
				case QSocketServer::Feature::Camera_ViewScale: Camera::get()->setViewScale(fValue); break;
				case QSocketServer::Feature::Camera_PosX_Reset: Camera::get()->resetX(); break;
				case QSocketServer::Feature::Camera_PosY_Reset: Camera::get()->resetY(); break;
				case QSocketServer::Feature::Camera_PosZ_Reset: Camera::get()->resetZ(); break;
				case QSocketServer::Feature::Camera_PitchAngle_Reset: Camera::get()->resetPitchAngle(); break;
				case QSocketServer::Feature::Camera_Fov_Reset: Camera::get()->resetFov(); break;
				case QSocketServer::Feature::Camera_YawAngle_Reset: Camera::get()->resetYawAngle(); break;
				case QSocketServer::Feature::Camera_ViewScale_Reset: Camera::get()->resetViewScale(); break;
				case QSocketServer::Feature::Camera_All_Reset: Camera::get()->resetAll(); break;
				default: {
					qWarning() << "invalid feature: " << feature;
					break;
				}
			}// end switch
			Cache::updateALC(alc);
			Cache::updateLDW(ldw);
			Cache::updateDCLC(dclc);
			Cache::updateAccTG(accTG);
			Cache::updateTailLamp(tailLamp);
		});
	}
};